import sys,os,socket,threading,struct,signal,json,re,serial,math,random,ast,datetime,time
from tadmin.models import Robot
from asgiref.sync import async_to_sync  
from AGV_WEB import consumers

#region # 自定义函数
def makdir(directory):
    if not os.path.exists(directory):  
            os.makedirs(directory) 
            print(directory+"不存在,已创建")
    # else: print(directory+"已存在")
def extract_ip():#获取本机IP
    st = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    try:
        st.connect(('10.255.255.255', 1))
        IP = st.getsockname()[0]
    except Exception:
        IP = '127.0.0.1'
    finally:
        st.close()
    return IP
def get_time():#获取本机时间
        current_time = datetime.datetime.now().strftime("%H:%M:%S.%f")[:-3]
        return current_time
def HexStrAddSpace(HexStr):#十六进制字符串标准化打印
    out_data = ''
    for i,s in enumerate(HexStr):
        if((i%2 == 0)&(i>0)):
            out_data += ' '
            out_data += s
        else:
            out_data += s
    return out_data.upper()
def listToFloat(list):#字符列表转小数
    return round(struct.unpack('<f', struct.pack('4B', *list))[0],5)#<表示小端字节序 f：这指定了要解析的数据类型为单精度浮点数（float）'4'：这表示接下来的格式字符将被重复四次。'B'：这是单个格式字符，表示无符号字符（unsigned char）。
    # return round(struct.unpack('>f', struct.pack('4B', *list))[0],5)
def listToFloat_B(list):#字符列表转小数
    # return round(struct.unpack('<f', struct.pack('4B', *list))[0],5)#<表示小端字节序 f：这指定了要解析的数据类型为单精度浮点数（float）'4'：这表示接下来的格式字符将被重复四次。'B'：这是单个格式字符，表示无符号字符（unsigned char）。
    return round(struct.unpack('>f', struct.pack('4B', *list))[0],5)
def floatToList(float_value):#小数转字符列表
    # 将浮点数转换为二进制表示
    # binary = struct.pack('!f', float_value)#参数'!f'表示使用大端字节序
    binary = struct.pack('f', float_value)
    # 将二进制表示转换为十六进
    hex_array = [int(b) for b in binary]#int整数表示  hex字符表示
    return hex_array
def floatToList_B(float_value):#小数转字符列表_大端
    # 将浮点数转换为二进制表示
    binary = struct.pack('!f', float_value)#参数'!f'表示使用大端字节序
    # binary = struct.pack('f', float_value)
    # 将二进制表示转换为十六进
    hex_array = [int(b) for b in binary]#int整数表示  hex字符表示
    return hex_array
def listToUint16(list):#字符列表转无符号32位整数
    return list[1]<<8 | list[0]
def listToInt16_big(list):#字符列表转有符号32位整数
    if list[0]>>7:
        return (list[0]<<8 | list[1])-0x10000
    else:return list[0]<<8 | list[1]
def listToInt16(list):#字符列表转有符号32位整数
    if list[1]>>7:
        return (list[1]<<8 | list[0])-0x10000
    else:return list[1]<<8 | list[0]
def listToInt32(arr):#将数组转为32位带符号的整数
    # byte_obj = bytes(arr)
    # num = int.from_bytes(byte_obj, 'little', signed=True)
    num = int.from_bytes(arr, 'big', signed=True)
    return num
def listToUint32(arr):#将数组转为32位无符号的整数
    num = int.from_bytes(arr, 'big', signed=False)
    return num
def listToInt32_little(arr):#将数组转为32位带符号的整数
    # byte_obj = bytes(arr)
    # num = int.from_bytes(byte_obj, 'little', signed=True)
    num = int.from_bytes(arr, 'little', signed=True)
    return num
def int32Tolist_big(num):
    return [num>>24&0xff,num>>16&0xff,num>>8&0xff,num&0xff]
def int32Tolist(num):
    return [num&0xff,num>>8&0xff,num>>16&0xff,num>>24&0xff]
def int16Tolist_big(num):
    return [num>>8&0xff,num&0xff]
def int16Tolist(num):
    return [num&0xff,num>>8&0xff]
def SumCheck(data,length):#按字节计算校验和
    checksum = 0
    for i in range(0, length):
        checksum += data[i]
        checksum &= 0xFF  # 强制截断
    return checksum
#endregion 
flag_reconnect = True
list_connectedDevice = []
class TcpService():
    def __init__(self):
        self.threads = [] 
        
    #region # TCP连接    
    def thread_tcp_service(self,device_id,device_ip,device_port):  
        # 创建socket对象  
        client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)  
        print(f"Connecting to {device_ip}:{device_port}") 
    
        # 连接到服务器  
        try:  
            client_socket.connect((device_ip,device_port))  
            list_connectedDevice.append({'device_id': device_id, 'socket': client_socket})
            Robot.objects.filter(id=device_id).update(status_connect = '已连接')
            print(f'设备{device_id}已连接')
            # print('已连接设备',list_connectedDevice)

            # 发送数据  
            message =f"{device_ip}:{device_port}已连接"
            client_socket.sendall(message.encode('gbk'))  
            try: 
                while True:
                    # 接收小于 1024 字节的数据  
                    data = client_socket.recv(1024) 
                    if not data:
                        break  
                    else:
                        self.fuc_data_process(device_id,data)
                        # pass
                        # print('Received from TCPserver')
                         
            except Exception as e:
                print(f"Error: {e}")
            finally:  
                # 关闭连接  
                client_socket.close() 
                if list_connectedDevice!= []:
                    # print('finally_list_connectedDevice',list_connectedDevice) 
                    print(f'移除socket:{device_id}')
                    list_connectedDevice.remove({'device_id': device_id, 'socket': client_socket})
                    # Robot.objects.filter(id=device_id).update(status_connect = '设备离线')
                # print('finally_list_connectedDevice',list_connectedDevice)
        except Exception as e:  
            print(f"Error: {e}")
            return False
    def start_tcp_thread(self,device_id,device_ip,device_port):
        # 创建线程
        # print("创建线程")
        thread = threading.Thread(target=self.thread_tcp_service, args=(device_id,device_ip,device_port))  
        thread.start()  
    def thread_check_connectStatus(self): # 检查连接状态
        # threads.append(thread)
        time.sleep(5)
        while flag_reconnect:
            # print("定时重连")
            time.sleep(3)
            
            obj = Robot.objects.values_list('id','device_ip', 'device_port','is_active','status_connect')
            obj_list = list(obj) 
            for obj in obj_list:  
                id = obj[0]
                ip = obj[1]
                port = obj[2] 
                is_active = obj[3]
                status_connect = obj[4]
                if is_active == True and status_connect == '设备离线':# 设备已激活但是离线，则重新连接
                    self.start_tcp_thread(id,ip,port) 
                elif is_active == True and status_connect == '已连接':# 设备已激活并且已连接,确认socket存在
                    flag_findDevice = False
                    for device_socket in list_connectedDevice:
                        if device_socket['device_id'] == id:
                            flag_findDevice = True
                            break
                    if flag_findDevice == False:
                        Robot.objects.filter(id=id).update(status_connect = '设备离线')
                elif is_active == False and status_connect == '设备离线':# 设备未激活但是离线，则不处理
                    pass
                elif is_active == False and status_connect == '已连接':# 设备未激活但是已连接，则移除并断开连接  
                    for device_socket in list_connectedDevice:
                        if device_socket['device_id'] == id:
                            device_socket['socket'].close()
                            list_connectedDevice.remove(device_socket)
                            break
                    Robot.objects.filter(id=id).update(status_connect = '设备离线')
        for device_socket in list_connectedDevice[::-1]:
            # print(f"关闭socket{device_socket['device_id']}")
            device_socket['socket'].close()
            Robot.objects.filter(id=device_socket['device_id']).update(status_connect = '设备离线')       
        print(f"结束定时重连线程 finally_list_connectedDevice:{list_connectedDevice}")
    def connect_tcpServer(self): 
        print("获取激活机器人列表")
        obj = Robot.objects.filter(is_active=True).values_list('id','device_ip', 'device_port')
        obj_list = list(obj)
        print(obj_list)
        # 遍历obj_list，尝试为每个对象建立TCP连接   
        for obj in obj_list:  
            id = obj[0]
            ip = obj[1]
            port = obj[2]       
            self.start_tcp_thread(id,ip,port) 
        # 此时，connections列表包含了所有成功建立的socket连接  
        # 注意：你应该在某个时刻关闭这些连接以避免资源泄露  
        # 例如，你可以在完成所有需要这些连接的操作后，遍历connections列表并调用sock.close()  
        thread = threading.Thread(target=self.thread_check_connectStatus)  
        thread.start() 
    #endregion
    #region # 接收数据处理
    def fuc_data_process(self,device_id,data):
        if(len(data)==137):
            return
            pass
            # self.server_parse_data(device_id,data)
        elif(len(data)==345):
            a=HexStrAddSpace(data.hex())   
        else:
            self.fuc_data_restore(device_id,data)
    def fuc_data_restore(self,device_id,data):# 接收数据还原（去转义）
        pattern = rb'(?<!\\);'# 如果';'前为'\'则不进行分割
        datalist = [x for x in re.split(pattern, data) if x]#去掉空''
        for i in datalist:                      
            pattern = rb'[\\](?=;)'
            i=re.sub(pattern,b"",i)#如果'\'后面是';'去掉'\'
            if len(i)>1:
                # print(len(data))
                self.fuc_data_classify(device_id,i) 
            else:print(data)
    def fuc_data_classify(self,device_id,data):# 接收数据分类
        # print(len(data))
        # try:
        if((data[0] == 0x50)&(data[1] == 0x43)):#PC（0x50,0x43）来自于上位机的数据
            self.fuc_data_verify(device_id,data)
        elif(data[0] == 0x50):#接收十六进制数组
            a=HexStrAddSpace(data.hex())
            self.fuc_log(device_id,a)
        elif((data[0] == 0x42)&(data[1] == 0x44)):#BD（0x42,0x44）来自于控制板的数据
            self.fuc_data_verify(device_id,data)
        elif((data[0] == 0x48)&(data[1] == 0x44)):#BD（0x42,0x44）来自于控制板的数据
            self.fuc_data_verify(device_id,data)
            # print(data)
            # a=HexStrAddSpace(data.hex())
            # self.fuc_log(a)
            # print(len(data))
        elif((data[0] == 0x00)&(data[1] == 0x44)):#华睿PGV相机原始数据解析
            self.fuc_log(device_id,HexStrAddSpace(data.hex()))  
            self.fuc_data_verify(device_id,data)
        elif((data[0] == 0x03)&(data[1] == 0x04)):#倍加福PGV相机原始数据解析
            self.fuc_log(device_id,HexStrAddSpace(data.hex()))  
            self.fuc_data_verify(device_id,data)
        elif((data[0] == 0x44)&(data[1] == 0x4D)):#DM显示在二维码界面
            return
            self.widget_tags.info.setText(get_time()+" "+data.decode('utf-8') )
        else: #接收中文或英文或其它
            # if self.check_showHex.isChecked():  
            #     a=HexStrAddSpace(data.hex())
            if False:  
                a=HexStrAddSpace(data.hex())
            else:                            
                try:
                    a=data.decode('utf-8')  
                    print("utf-8")                          
                except Exception:
                    try:
                        a=data.decode('gbk')
                        print("gbk")
                    except : 
                        a=str(data)
                        print("字符串")
            self.fuc_log(device_id,a)  
        # except:
        #     self.fuc_log("接收数据分类错误")
    def fuc_data_verify(self, device_id,data):# 接收数据验证   
        # print(len(data)) 
        # if(len(data)>3):
        try:
            if((len(data)>8)&(len(data)==data[3])&(data[len(data)-1]==0xda)):#帧头、帧尾、长度都对
                # print("单帧")
                self.fuc_data_apply(device_id,data[4:len(data)-2]) 
                # a=HexStrAddSpace(data.hex())#查看原始数据
                # self.fuc_log(a)
            elif((len(data)<300)&(len(data)>data[3])):#多帧数据拆分开处理
                processData=data[:data[3]]
                # self.fuc_data_verify(processData) #!!!数据合格分开后，重新验证
                self.fuc_data_verify(device_id,processData) #!!!数据合格分开后，重新验证，不能直接调用fuc_data_apply！！！
                remainData=data[data[3]:]   
                # print("多帧")
                # print("总长度"+str(len(data))+'长度'+str(data[3])) 
                self.fuc_data_classify(device_id,remainData)
                # print("剩余长度"+str(len(remainData)))
            else:
                if(data[len(data)-1]!=0xda):
                    # self.fuc_log("帧尾错误")
                    try:print(data);a=data.decode('utf-8');print("english")                        
                    except Exception:
                        try:
                            # a=data.decode('gbk')
                            a=HexStrAddSpace(data.hex())
                            print("中文")
                        except : a=str(data)
                    # self.fuc_log(HexStrAddSpace(data.hex()))  
                    self.fuc_log(device_id,a) 
                else:
                    print("数据格式错误")
                    # self.fuc_data_apply(data[4:len(data)-2])   
        except:
            a=HexStrAddSpace(data.hex())
            self.fuc_log(device_id,a)
    def fuc_data_apply(self, device_id,data):# 接收数据应用    
        if((data[0]==0x40)&(data[1]==0x01)):#上位机监控数据
            data=data[2:]  
            # print(len(data))        
            agv_currentLineSpeed=listToFloat(data[0:4])
            agv_currentRateSpeed=listToFloat(data[4:8])
            agv_currentPosX=round(listToFloat(data[16:20]),3)
            agv_currentPosY=round(listToFloat(data[20:24]),3)
            agv_currentAngle=round(listToFloat(data[24:28]),3)  
            if agv_currentLineSpeed=='':agv_currentLineSpeed=0
            if agv_currentRateSpeed=='':agv_currentRateSpeed=0
            msgType='msgType_DeviceStatus'
            msg={
                '机器人信息':{'pos':f'( {agv_currentPosX} , {agv_currentPosY} )','angle':agv_currentAngle,'lineSpeed':agv_currentLineSpeed,'rateSpeed':agv_currentRateSpeed},
                'imuState':{'pos':device_id,'angle':30,'speed':50},
                'msgType':msgType,
            }
            async_to_sync(consumers.broadcast_message)(device_id,msgType,msg)
        return
        if((data[0]==0x00)&(data[1]==0x00)):#升级进度
            data=data[2:]
            if data[0]==0xff:
                self.fuc_log("升级失败")
            else:
                lenth=data[0]<<24|data[1]<<16|data[2]<<8|data[3]
                try:
                    self.pgB_upgrade.setValue(lenth)  
                except Exception:
                    self.fuc_log("返回数据错误")
                if self.fileLen==lenth:
                    self.fuc_log("升级完成")
        elif((data[0]==0x12)&(data[1]==0x01)):#士腾控制板程序监测
            agv_data1=data[2:]
            self.thread_debug.fuc_updateData1(agv_data1)
        elif((data[0]==0x12)&(data[1]==0x02)):#士腾控制板程序监测
            agv_data2=data[2:]
            self.thread_debug.fuc_updateData2(agv_data2)
        elif((data[0]==0x30)&(data[1]==0x00)):#上报芯片UID
            data=data[2:] 
            uid1=data[:4][::-1].hex()  
            uid2=data[4:8][::-1].hex()  
            uid3=data[8:12][::-1].hex()  
            self.input_license_id1.setText(uid1)
            self.input_license_id2.setText(uid2)
            self.input_license_id3.setText(uid3)
        elif((data[0]==0x30)&(data[1]==0x01)):#上报芯片XID(UID偏移得到)
            data=data[2:] 
            uid1=data[:4][::-1].hex()  
            uid2=data[4:8][::-1].hex()  
            uid3=data[8:12][::-1].hex()  
            self.input_license_xid1.setText(uid1)
            self.input_license_xid2.setText(uid2)
            self.input_license_xid3.setText(uid3)
            # print("设备ID:"+HexStrAddSpace(uid1.hex()+uid2.hex()+uid3.hex()))
            # a=HexStrAddSpace(data.hex())
            # self.fuc_log(a) 
        elif((data[0]==0x30)&(data[1]==0x02)):#上报自身ID
            data=data[2:] 
            self.label_device_id.setText(str(data[3]<<8|data[4]).zfill(4))
            # self.fuc_log("设备ID:"+HexStrAddSpace(data.hex()))
        elif((data[0]==0x40)&(data[1]==0x01)):#上位机监控数据
            data=data[2:]  
            # print(len(data))        
            agv_currentLineSpeed=listToFloat(data[0:4])
            agv_currentRateSpeed=listToFloat(data[4:8])
            agv_currentPosX=round(listToFloat(data[16:20]),3)
            agv_currentPosY=round(listToFloat(data[20:24]),3)
            agv_currentAngle=round(listToFloat(data[24:28]),3)  
            msgType='msgType_Log'
            async_to_sync(consumers.broadcast_message)(device_id,msgType,text)
            self.input_agv_curentLineSpeed.setText(str(agv_currentLineSpeed))
            self.input_agv_curentRateSpeed.setText(str(agv_currentRateSpeed))
            self.input_agv_currentPosX.setText(str(agv_currentPosX))#世界坐标X信息
            self.input_agv_currentPosY.setText(str(agv_currentPosY))#世界坐标Y信息
            self.input_agv_curentAngle.setText(str(agv_currentAngle))#角度信息
          
            self.thread_position.update(agv_currentPosX,agv_currentPosY)# 更新运动轨迹          
            self.thread_angle.fuc_updateAngle(agv_currentAngle)# 更新角度曲线的数据(通过线程处理数据(数组左移)）
            if(len(data)>=32):#超神波距离信息
                agv_ultrasonic=listToFloat(data[28:32])
                self.input_agv_ultrasonic.setText(str(agv_ultrasonic))
            if(len(data)>=34):#电池电压信息
                 battery_voltage=(data[32]<<8|data[33])/1000
                 self.input_battery_voltage.setText(str(battery_voltage))
            if(len(data)>=36):#电池剩余电量信息
                battery_leftEnergy=data[34]<<8|data[35]
                self.input_battery_energy.setText(str(battery_leftEnergy))
            if(len(data)>=40):#左电机反馈速度
                motorLeftSpeed=listToFloat(data[36:40])
                self.input_motorL_speed.setText(str(motorLeftSpeed))
            if(len(data)>=44):#右电机反馈速度
                motorRightSpeed=listToFloat(data[40:44])
                self.input_motorR_speed.setText(str(motorRightSpeed))
            if(len(data)>=48):#左电机反馈位置
                motorLeftPosition=listToFloat(data[44:48])
                self.input_motorL_position.setText(str(motorLeftPosition))
            if(len(data)>=52):#右电机反馈位置
                motorRightPosition=listToFloat(data[48:52])
                self.input_motorR_position.setText(str(motorRightPosition))
            if(len(data)>=56):#旋转电机反馈速度
                motorRotatorSpeed=listToFloat(data[52:56])
                self.input_motorT_speed_2.setText(str(motorRotatorSpeed))
            if(len(data)>=60):#顶升电机反馈速度
                motorUpSpeed=listToFloat(data[56:60])
                self.input_motorU_speed.setText(str(motorUpSpeed))
                self.input_motorU_speed_2.setText(str(motorUpSpeed))
            if(len(data)>=64):#旋转电机反馈位置
                motorRotatorPosition=listToFloat(data[60:64])
                self.input_motorT_position.setText(str(motorRotatorPosition))
                self.input_motorT_position_2.setText(str(motorRotatorPosition))
            if(len(data)>=68):#顶升电机反馈位置
                motorUpPosition=listToFloat(data[64:68])
                self.input_motorU_position.setText(str(motorUpPosition))
                self.input_motorU_position_2.setText(str(motorUpPosition))
            if(len(data)>=71):#输入状态反馈
                inputor=data[68:72]
                for i in range(8):
                    self.led_inputors[i].show(inputor[0]>>(7-i)&0x01)
                for i in range(8,16):
                    self.led_inputors[i].show(inputor[1]>>(15-i)&0x01)
                for i in range(16,24):
                    self.led_inputors[i].show(inputor[2]>>(23-i)&0x01)
                for i in range(24,28):
                    self.led_inputors[i].show(inputor[3]>>(31-i)&0x01)
                # print(inputor)
                # self.led_inputor_1.show(inputor[0]>>7&0x01)
                # self.led_inputor_2.show(inputor[0]>>6&0x01)
                # self.led_inputor_3.show(inputor[0]>>5&0x01)
                # self.led_inputor_4.show(inputor[0]>>4&0x01)
                # self.led_inputor_5.show(inputor[0]>>3&0x01)
                # self.led_inputor_6.show(inputor[0]>>2&0x01)
                # self.led_inputor_7.show(inputor[0]>>1&0x01)
                # self.led_inputor_8.show(inputor[0]>>0&0x01)

                # self.led_inputor_9.show(inputor[1]>>7&0x01)
                # self.led_inputor_10.show(inputor[1]>>6&0x01)
                # self.led_inputor_11.show(inputor[1]>>5&0x01)
                # self.led_inputor_12.show(inputor[1]>>4&0x01)
                # self.led_inputor_13.show(inputor[1]>>3&0x01)
                # self.led_inputor_14.show(inputor[1]>>2&0x01)
                # self.led_inputor_15.show(inputor[1]>>1&0x01)
                # self.led_inputor_16.show(inputor[1]>>0&0x01)

                # self.led_inputor_17.show(inputor[2]>>7&0x01)
                # self.led_inputor_18.show(inputor[2]>>6&0x01)
                # self.led_inputor_19.show(inputor[2]>>5&0x01)
                # self.led_inputor_20.show(inputor[2]>>4&0x01)
                # self.led_inputor_21.show(inputor[2]>>3&0x01)
                # self.led_inputor_22.show(inputor[2]>>2&0x01)
                # self.led_inputor_23.show(inputor[2]>>1&0x01)
                # self.led_inputor_24.show(inputor[2]>>0&0x01)

                # self.led_inputor_25.show(inputor[3]>>7&0x01)
                # self.led_inputor_26.show(inputor[3]>>6&0x01)
                # self.led_inputor_27.show(inputor[3]>>5&0x01)
                # self.led_inputor_28.show(inputor[3]>>4&0x01)
            # print('hlly')

        elif((data[0]==0x40)&(data[1]==0x02)):#控制板数据测试预留
            data=data[2:]#测试预留
        elif((data[0]==0x40)&(data[1]==0x03)):#更新计算目标角度
            data=data[2:]
            angle=[data[0],data[1],data[2],data[3]]
            angle=listToFloat(angle)
            self.input_agv_targetAngle.setText(str(angle))       
        elif((data[0]==0x40)&(data[1]==0x04)):#更新计算目标距离
            data=data[2:]
            targetDistance=[data[0],data[1],data[2],data[3]]
            targetDistance=listToFloat(targetDistance)
            self.input_agv_targetDistance.setText(str(targetDistance))
        elif((data[0]==0x40)&(data[1]==0x05)):#更新PGV相机信息
            data=data[2:]
            # x
            PGV_posX=data[0]<<24|data[1]<<16|data[2]<<8|data[3]
            if(data[0]>>7):PGV_posX=PGV_posX-0x100000000
            # y
            PGV_posY=data[4]<<8|data[5]
            if(data[4]>>7):PGV_posY=PGV_posY-0x10000
            # w
            PGV_posYaw=data[6]<<8|data[7]
            if(data[6]>>7):PGV_posYaw=PGV_posYaw-0x10000
            PGV_posYaw=round(PGV_posYaw/10,1)
            # number
            PGV_tagNuber=data[8]<<24&0xff|data[9]<<16&0xff|data[10]<<8&0xff|data[11]&0xff

            # 更新PGV数据
            self.input_PGV_posX.setText(str(PGV_posX))
            self.input_PGV_posY.setText(str(PGV_posY))
            self.input_PGV_posYaw.setText(str(PGV_posYaw))
            self.input_PGV_tagNuber.setText(str(PGV_tagNuber))
            # 更新角度曲线的数据
            # self.angleCurve.y1[:-1] = self.angleCurve.y1[1:]
            # self.angleCurve.y1[-1] = PGV_posYaw
            # self.angleCurve.plot1.setData(self.angleCurve.x, self.angleCurve.y1)
        elif((data[0]==0x40)&(data[1]==0x06)):#更新关键点
            data=data[2:]
            agv_currentPosX=listToFloat(data[0:4])
            agv_currentPosY=listToFloat(data[4:8])
            #更新运动轨迹
            # if((self.last_agv_currentPosX!=agv_currentPosX)|(self.last_agv_currentPosY!=agv_currentPosY)):
            #     self.paint.dotdata.append({'pos': (agv_currentPosX, agv_currentPosY),'brush': 'r','pen':'r'})  # 新增点
            #     self.paint.scatter.setData(self.paint.dotdata)
            # self.last_agv_currentPosX=agv_currentPosX
            # self.last_agv_currentPosY=agv_currentPosY
        elif((data[0]==0x40)&(data[1]==0x07)):#监测陀螺仪原始数据
            imuRawData=data[2:]
            self.thread_angle.fuc_updateImuRaw(imuRawData)# 更新角度曲线的数据(通过线程处理数据(数组左移)）

        elif((data[0]==0x40)&(data[1]==0x10)):#读取flash参数1-7
            data=data[2:]              
            if len(data)==(len(self.agv_parameter))*4:#每个参数占4个字节
                for i in range(len(self.agv_parameter)):
                    found_widget = self.widget_agvParameter.findChild(QLineEdit,self.agv_parameter[i])#根据字符串查找widget中的对象    
                    if i==0:#第一个参数为版本号，是整数不是小数
                        found_widget.setText(str(data[4*i]))
                    else:
                        found_widget.setText(str(listToFloat(data[4*i:4+4*i])))
                self.fuc_log('参数读取完成')
                self.btn_read_flashParameter.setText("读取参数")
            else:print('接收长度错误')
        elif((data[0]==0x40)&(data[1]==0x11)):#读取flash参数8-14
            data=data[2:]  
            # print(data)        
            imu_integralGain=listToFloat(data[0:4])
            imu_GyroOffset_Z=listToFloat(data[4:8])
            agv_angleRangeOffset=listToFloat(data[8:12])
            agv_distanceRangeOffset=listToFloat(data[12:16])
            factor_rectify=listToFloat(data[16:20])
            rectifyAngle=listToFloat(data[20:24])
            self.input_imu_integralGain.setText(str(imu_integralGain))
            self.input_imu_offsetZ.setText(str(imu_GyroOffset_Z))
            self.input_angleRangeOffset.setText(str(agv_angleRangeOffset))
            self.input_distanceRangeOffset.setText(str(agv_distanceRangeOffset))
            self.input_factor_rectify.setText(str(factor_rectify))
            self.input_rectifyAngle.setText(str(rectifyAngle))
            self.btn_read_flashParameter.setText("读取参数")
        elif((data[0]==0x40)&(data[1]==0x12)):#读取flash参数15-21
            data=data[2:]  
            # print(data)        
            leftWheel_compensation=listToFloat(data[0:4])
            rightWheel_compensation=listToFloat(data[4:8])
            leftWheel_offset=listToFloat(data[8:12])
            rightWheel_offset=listToFloat(data[12:16])
            self.input_LW_compensation.setText(str(leftWheel_compensation))
            self.input_RW_compensation.setText(str(rightWheel_compensation))
            self.input_LW_offset.setText(str(leftWheel_offset))
            self.input_RW_offset.setText(str(rightWheel_offset))
        elif((data[0]==0x40)&(data[1]==0x20)):#接收二维码标定数据
            data=data[2:]
            DM_tagNumber=listToInt32_little(data[0:4])
            DM_posX=listToFloat(data[4:8])
            DM_posY=listToFloat(data[8:12])
            DM_ErrorAngle=listToFloat(data[12:])

            # 创建新页面
            if self.DMcode_number%self.page_codeNum==0:
                self.new_page = QWidget()
                self.grid_layout = QGridLayout()
                self.new_page.setLayout(self.grid_layout)
                self.widget_tags.stack_tags.addWidget(self.new_page)
                # labelList=["","序号","二维码编码","坐标X","坐标Y","补偿角度"]
                labelList=["","序号","二维码编码","坐标X","坐标Y","补偿角度"]
                for i in range(3):
                    for j in range(6):
                        label=QLabel(labelList[j])
                        self.grid_layout.addWidget(label,0,6*i+j)

            # 创建输入框
            check=QCheckBox()
            check.setChecked(True)
            self.checkBoxs.append(check)
            label=QLabel(str(self.DMcode_number+int(self.widget_tags.input_start_address.text())))
            self.dmcode_labels.append(label)
            self.grid_layout.addWidget(check,self.DMcode_number%self.page_codeNum//3+1,6*(self.DMcode_number%self.page_codeNum%3))
            self.grid_layout.addWidget(label,self.DMcode_number%self.page_codeNum//3+1,6*(self.DMcode_number%self.page_codeNum%3)+1)
            for j in range(4):
                line_edit = QLineEdit(self)
                line_edit.setObjectName(f'line_edit_{4*self.DMcode_number+j}')
                self.line_edits.append(line_edit)
                self.grid_layout.addWidget(line_edit,self.DMcode_number%self.page_codeNum//3+1,6*(self.DMcode_number%self.page_codeNum%3)+j+2)

            self.line_edits[4*self.DMcode_number].setText(str(DM_tagNumber))
            self.line_edits[4*self.DMcode_number+1].setText(str(DM_posX))
            self.line_edits[4*self.DMcode_number+2].setText(str(DM_posY))
            self.line_edits[4*self.DMcode_number+3].setText(str(DM_ErrorAngle))
            self.DMcode_number+=1
        elif((data[0]==0x42)&(data[1]==0x00)):#更新扩展输入口状态
                self.check_input_1.setChecked(True if ((data[2]&0x01)>0) else False)
                self.check_input_2.setChecked(True if ((data[2]&0x02)>0) else False)
                self.check_input_3.setChecked(True if ((data[2]&0x04)>0) else False)
                self.check_input_4.setChecked(True if ((data[2]&0x08)>0) else False)
                self.check_input_5.setChecked(True if ((data[2]&0x10)>0) else False)
                self.check_input_6.setChecked(True if ((data[2]&0x20)>0) else False)
                self.check_input_7.setChecked(True if ((data[2]&0x40)>0) else False)
                self.check_input_8.setChecked(True if ((data[2]&0x80)>0) else False)
        elif((data[0]==0x42)&(data[1]==0x09)):#CAN通信反馈(电机、IMU)
            self.fuc_log(HexStrAddSpace(data[2:].hex()))  
            canId=data[2]<<8|data[3]
            canData=data[4:]
            if(canId==0x700+int(self.input_motor_nodeId.text())):               
                if(canData[0]==0x00):self.label_motor_heartState.setText("启动状态")
                if(canData[0]==0x02):self.label_motor_heartState.setText("连接状态")
                if(canData[0]==0x04):self.label_motor_heartState.setText("停止状态")
                if(canData[0]==0x05):self.label_motor_heartState.setText("操作状态")
                if(canData[0]==0x7F):
                    self.label_motor_heartState.setText("预操作状态")
                    self.label_motor_active.setText("已激活")
            # if(canId==0x180+int(self.input_motor_nodeId.text())): 
            #     speed=canData[1]<<8|canData[0]
            #     if(canData[1]>>7):speed-=0x10000
            #     self.label_motor_speed.setText(str(int(speed))+'rpm')
            if(canId==0x585):# 陀螺仪数据
                if(canData[3]==0X01):
                    angle=[canData[4],canData[5],canData[6],canData[7]]
                    angle=listToFloat(angle)
                    self.input_agv_curentAngle.setText(str(angle))
                if(canData[3]==0X02):
                    anglerRate=canData[4]+canData[5]<<8+canData[6]<<16+canData[7]<<24
                    
                    print(anglerRate)
                    # self.input_imu_angleRate.setText(str(anglerRate)+'rad/s')       
        elif((data[0]==0x42)&(data[1]==0x20)):#处理读码相机数据
            self.sigCall_process_PGVData(data[2:])
        elif((data[0]==0x70)&(data[1]==0x01)):#麦克纳姆小车_轮速
            data=data[2:]
            self.thread_mecanum.fuc_updateData(data)
        elif((data[0]==0x70)&(data[1]==0x02)):#麦克纳姆小车_PID参数
            data=data[2:]
            for i in range(12):
                self.pid_inputs[i].setText(str(listToFloat_B(data[4*i:4*i+4])))
        elif((data[0]==0x80)&(data[1]==0x01)):#手柄采样数据
            data=data[2:]
            lx=listToInt16(data[0:2])
            ly=listToInt16(data[2:4])
            rx=listToInt16(data[4:6])
            ry=listToInt16(data[6:8])
            self.widget_handle.progress_scale_LX.setValue(lx)
            self.widget_handle.progress_scale_LY.setValue(ly)
            self.widget_handle.progress_scale_RX.setValue(rx)
            self.widget_handle.progress_scale_RY.setValue(ry)
            self.widget_handle.line_scale_LX.setText(str(lx))
            self.widget_handle.line_scale_LY.setText(str(ly))
            self.widget_handle.line_scale_RX.setText(str(rx))
            self.widget_handle.line_scale_RY.setText(str(ry))
            # print("lx: ",lx,"ly: ",ly,"rx: ",rx,"ry: ",ry)
            if self.flag_calibrate_scale:
                lx_min=int(self.widget_handle.line_scale_LXmin.text())
                lx_max=int(self.widget_handle.line_scale_LXmax.text())
                ly_min=int(self.widget_handle.line_scale_LYmin.text())
                ly_max=int(self.widget_handle.line_scale_LYmax.text())
                rx_min=int(self.widget_handle.line_scale_RXmin.text())
                rx_max=int(self.widget_handle.line_scale_RXmax.text())
                ry_min=int(self.widget_handle.line_scale_RYmin.text())
                ry_max=int(self.widget_handle.line_scale_RYmax.text())
                
                if lx<lx_min:self.widget_handle.line_scale_LXmin.setText(str(lx))
                if lx>lx_max:self.widget_handle.line_scale_LXmax.setText(str(lx))
                if ly<ly_min:self.widget_handle.line_scale_LYmin.setText(str(ly))
                if ly>ly_max:self.widget_handle.line_scale_LYmax.setText(str(ly))
                if rx<rx_min:self.widget_handle.line_scale_RXmin.setText(str(rx))
                if rx>rx_max:self.widget_handle.line_scale_RXmax.setText(str(rx))
                if ry<ry_min:self.widget_handle.line_scale_RYmin.setText(str(ry))
                if ry>ry_max:self.widget_handle.line_scale_RYmax.setText(str(ry))

        elif((data[0]==0x80)&(data[1]==0x02)):#手柄采样数据
            data=data[2:]
            lx_min=listToInt16(data[0:2])
            ly_min=listToInt16(data[2:4])
            rx_min=listToInt16(data[4:6])
            ry_min=listToInt16(data[6:8])
            lx_max=listToInt16(data[8:10])
            ly_max=listToInt16(data[10:12])
            rx_max=listToInt16(data[12:14])
            ry_max=listToInt16(data[14:16])
            # self.custom_rect1.setColorRange(lx_min,lx_max)
            # print(lx_min,ly_min)
            # print(adc1,adc2,adc3,adc4)
        else:
            a=HexStrAddSpace(data.hex())
            self.fuc_log(a)
    def server_parse_data(self,device_id,data):# 解析调度系统数据
        # print(len(data))
        Distribution_code=data[12:16]
        if Distribution_code==b'AFLF':#验证接收数据
            CMD_STATE=list(data[24:64])
            # for i in range(20):#数据存储大小端交换
            #     CMD_STATE[2*i:2*i+2]=CMD_STATE[2*i:2*i+2][::-1]
            CMD_STATE_note=["未开始"," 开始 ","执行中"," 完成 "," 错误 "]
            CMD_STATE_color=["grey"," black","blue"," green"," red"]
            
            for i in range(10):
                # cmdNum=CMD_STATE[0+4*i]*16+CMD_STATE[1+4*i]#data[0]data[1]为返回的状态序号
                cmdNum=CMD_STATE[0+4*i]<<8|CMD_STATE[1+4*i]#data[0]data[1]为返回的状态序号
                cmdState=CMD_STATE[3+4*i]
                widget = self.gridLayout_25.itemAtPosition(i, 0).widget()
                widget.setText(str(cmdNum))
                widget = self.gridLayout_25.itemAtPosition(i, 1).widget()
                widget.setText(CMD_STATE_note[cmdState])
                widget.setStyleSheet("color:"+CMD_STATE_color[cmdState])#根据状态号设置状态颜色

                # for j in range(self.num_cmd):
                #     widget = self.layout_cmd.itemAt(11+(0)*self.num_cmd+j).widget()
                #     if widget.isChecked():#判断复选框是否被选中
                #         widget = self.layout_cmd.itemAt(11+(1)*self.num_cmd+j).widget()
                #         if widget.text()==str(cmdNum):#判断命令号相同,默认初始反馈的10条命令命令号均为0时，下发命令号为0的命令，状态会被其他命令号为0的状态覆盖
                #             widget = self.layout_cmd.itemAt(11+(10)*self.num_cmd+j).widget()
                #             widget.setText(CMD_STATE_note[cmdState])#data[3]为返回的状态命令位，根据状态号设置状态名字
                #             widget.setStyleSheet("color:"+CMD_STATE_color[cmdState])#根据状态号设置状态颜色

            posX=listToInt32(data[64:68])                 #当前位置X            
            posY=listToInt32(data[68:72])                 #当前位置Y            
            angle=listToInt32(data[72:76])                #当前角度            
            speed=listToInt32(data[76:80])                #当前线速度            
            nav_mode=0#导航模式            
            lift=0#顶升            
            tab_degree=0#转盘角度            
            reserve_degree=0#预留角度            
            RFID_last=listToUint16(data[89:91]) #上一个RFID            
            # tag_number=listToInt32(data[91:93]) #当前RFID            
            RFID=listToUint16(data[91:93]) #当前RFID            
            speed_x=0#X方向速度            
            speed_y=0#X方向速度            
            rateSpeed=listToInt32(data[100:104])          #当前角速度            
            Errors=[]#错误位            
            Warn=[]#警告 位            
            Status=[]#状态位            
            battery=listToInt32(data[124:126])            #当前电池电量百分比           
            capacity=0#当前电容电量百分比            
            odometer=0#里程计           
            BAT_health=0#电池健康度            
            Reserved=0#预留
            self.input_fb_posX.setText(str(posX))
            self.input_fb_posY.setText(str(posY))
            self.input_fb_angle.setText(str(angle))
            self.input_fb_speed.setText(str(speed))
            self.input_fb_battery.setText(str(battery)+'%')
            self.input_fb_rateSpeed.setText(str(rateSpeed))
            self.input_fb_Status.setText(str(Status))
            self.input_fb_Errors.setText(str(Errors))
            self.input_fb_RFID.setText(str(RFID))


            #更新运动轨迹
            agv_currentPosX=posX/1000
            agv_currentPosY=posY/1000
            self.thread_position.update(agv_currentPosX,agv_currentPosY)# 更新运动轨迹 
            # if((self.last_agv_currentPosX!=agv_currentPosX)|(self.last_agv_currentPosY!=agv_currentPosY)):
            #     self.paint.dotdata.append({'pos': (agv_currentPosX, agv_currentPosY),'brush': pg.mkBrush('g'),'pen':pg.mkPen(color='g')})  # 设置每个点的颜色
            #     self.paint.scatter.setData(self.paint.dotdata)
            # self.last_agv_currentPosX=agv_currentPosX
            # self.last_agv_currentPosY=agv_currentPosY
            # print(data[64],data[65],data[66],data[67])
        # flag_cmd_cycle=1    
        # for i in range(self.cmd_count):
        #     if CMD_STATE[2+4*i]!=4:
        #         flag_cmd_cycle=0
        # if (flag_cmd_cycle==1)&(self.check_cmd_cycle.isChecked()):
        #     self.fuc_sendCmd_all()
        # AGV_STATE=data[64:96]
        # # self.signal.print.emit('收到数据: %s' % AGV_STATE)
        # self.input_fb_posX.setText(str(int.from_bytes(AGV_STATE[:4], byteorder='little',signed=True)/10))
        # self.input_fb_posY.setText(str(int.from_bytes(AGV_STATE[4:8], byteorder='little',signed=True)/10))
        # self.input_fb_angle.setText(str(int.from_bytes(AGV_STATE[8:10], byteorder='little',signed=True)/10))
        # self.input_fb_speed.setText(str(int.from_bytes(AGV_STATE[12:16], byteorder='little',signed=True)/10))
        # self.input_fb_angle.setText(str(struct.unpack('<H', AGV_STATE[8:10])[0]))
    #endregion
    #region # 日志管理
    def fuc_log(self,device_id, text=""):#日志显示区打印
        # 定义ANSI转义序列  
        red = '\x1b[31m'  
        yellow = '\x1b[33m'  
        blue = '\x1b[34m'  
        reset = '\x1b[0m'  # 重置颜色
        text=get_time()+"  "+text    
        text=f"{red}{text}{reset}"
        msgType='msgType_Log'
        async_to_sync(consumers.broadcast_message)(device_id,msgType,text)
    def sigCall_print(self, text=""):
        log=get_time()+"  "+text
        self.textBrowser.append(log) 
        self.fuc_savelog(log) 
    def fuc_openlog(self):#打开日志
        pass
        # filename, _ = QFileDialog.getOpenFileName(None, 'Open File', os.getcwd()+"/appDoc/log")
        # if filename:os.startfile(filename)
    def fuc_savelog(self,text):#保存日志
        if self.check_record.isChecked():
            filepath="appDoc/log/{0}.txt".format(self.input_record_name.text()) 
            with open(filepath, "a+",encoding='utf-8') as f:
                f.write("{}\n".format(text))  # 自带文件关闭功能，不需要再写f.close()
    def fuc_clear(self, text=""):#日志显示区清空
        self.textBrowser.clear()
        # self.fuc_log("hlly\r\n")
    #endregion  
#region # 程序退出时执行清理操作
def cleanup_function():  
    # 清理代码  
    print("执行清理操作")  
    global flag_reconnect
    flag_reconnect = False
def signal_handler(sig, frame):  
    print('你按下了Ctrl+C!')  
    # 调用清理函数  
    cleanup_function()  
    # 退出程序  
    sys.exit(0)  
signal.signal(signal.SIGINT, signal_handler)  # 设置SIGINT的信号处理函数  
#endregion

#region # 启动后台Tcp服务
from asgiref.sync import async_to_sync
tcp=TcpService()
tcp.connect_tcpServer()
#endregion
#region # redis测试
import channels.layers

channel_layer = channels.layers.get_channel_layer()
async_to_sync(channel_layer.send)('test_channel', {'type': 'hello'})
result=async_to_sync(channel_layer.receive)('test_channel')
print(f"redis测试:{result}")
#endregion